#include <ros/ros.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv) {
    ros::init(argc, argv, "initial_pose_publisher");
    ros::NodeHandle nh;
    ros::NodeHandle private_nh("~");
    
    // 获取参数
    double x, y, z, roll, pitch, yaw;
    private_nh.param("x", x, 0.0);
    private_nh.param("y", y, 0.0);
    private_nh.param("z", z, 0.0);
    private_nh.param("roll", roll, 0.0);
    private_nh.param("pitch", pitch, 0.0);
    private_nh.param("yaw", yaw, 0.0);
    
    // 创建发布器
    ros::Publisher pose_pub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 1, true);
    
    // 等待连接
    ros::Duration(1.0).sleep();
    
    // 创建初始位姿消息
    geometry_msgs::PoseWithCovarianceStamped initial_pose;
    initial_pose.header.frame_id = "map";
    initial_pose.header.stamp = ros::Time::now();
    
    // 设置位置
    initial_pose.pose.pose.position.x = x;
    initial_pose.pose.pose.position.y = y;
    initial_pose.pose.pose.position.z = z;
    
    // 设置方向（使用四元数）
    tf::Quaternion q;
    q.setRPY(roll, pitch, yaw);
    initial_pose.pose.pose.orientation.x = q.x();
    initial_pose.pose.pose.orientation.y = q.y();
    initial_pose.pose.pose.orientation.z = q.z();
    initial_pose.pose.pose.orientation.w = q.w();
    
    // 设置协方差（使用单位矩阵）
    for (int i = 0; i < 36; i++) {
        initial_pose.pose.covariance[i] = 0.0;
    }
    for (int i = 0; i < 6; i++) {
        initial_pose.pose.covariance[i*6+i] = 0.1;  // 对角线元素设为0.1
    }
    
    // 发布初始位姿
    pose_pub.publish(initial_pose);
    ROS_INFO("Published initial pose: x=%.2f, y=%.2f, z=%.2f, roll=%.2f, pitch=%.2f, yaw=%.2f",
             x, y, z, roll, pitch, yaw);
    
    // 等待一段时间，确保消息被接收
    ros::Duration(1.0).sleep();
    
    return 0;
} 